/*NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

	Copyright (c) 2009 Jounayd Id Salah.
	All rights reserved.

	This file is part of Blacklib.

	Blacklib is free software: you can redistribute it and/or modify it under the 
	terms of the GNU Lesser General Public License as published by the Free Software 
	Foundation, either version 3 of the License, or (at your option) any later version.
	Blacklib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; 
	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
	PURPOSE.  See the GNU Lesser General Public License for more details.
	You should have received a copy of the GNU Lesser General Public License along with 
	Blacklib.  If not, see <http://www.gnu.org/licenses/>.
*/
//NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

#include "maths/quaternion.h"
#include "maths/matrix.h"

namespace black
{
	//***************************************************************
	// quat
	//***************************************************************

	//__________________________________________________________________________
	const quat quat::ZERO			= quat( 0.f, 0.f, 0.f, 1.f );

	//__________________________________________________________________________
	/*! coming from J.M.P. van Waveren from Id Software */
	quat::quat( const mat3x3_128& m )
	{
		assert( m.isValid() );
		if ( m._.ix + m._.jy + m._.kz > 0.0f )
		{
			const float t = m._.ix + m._.jy + m._.kz + 1.f;
			const float s = 0.5f * fastInvSqrt( t );

			w = t * s;
			z = (m._.iy - m._.jx) * s;
			y = (m._.kx - m._.iz) * s;
			x = (m._.jz - m._.ky) * s;
		}
		else if ( m._.ix > m._.jy && m._.ix > m._.kz )
		{
			const float t = m._.ix - m._.jy - m._.kz + 1.f;
			const float s = 0.5f * fastInvSqrt( t );

			x = t * s;
			y = (m._.iy + m._.jx) * s;
			z = (m._.kx + m._.iz) * s;
			w = (m._.jz - m._.ky) * s;
		}
		else if ( m._.jy > m._.kz )
		{
			const float t = - m._.ix + m._.jy - m._.kz + 1.f;
			const float s = 0.5f * fastInvSqrt( t );

			y = t * s;
			x = (m._.iy + m._.jx) * s;
			w = (m._.kx - m._.iz) * s;
			z = (m._.jz + m._.ky) * s;
		}
		else
		{
			const float t = - m._.ix - m._.jy + m._.kz + 1.f;
			const float s = 0.5f * fastInvSqrt( t );

			z = t * s;
			w = (m._.iy - m._.jx) * s;
			x = (m._.kx + m._.iz) * s;
			y = (m._.jz + m._.ky) * s;
		}
		assert( isValid() );
	}

	//__________________________________________________________________________
	quat& quat::slerp( const quat& q1, const quat& q2, const float t )
	{
		static const float	fQuatEpsilon = fEPSILON * fEPSILON;

		*this = q1;

		float fCos = x*q2.x + y*q2.y + z*q2.z + w*q2.w;

		float fSign = 1.f;
		if ( fCos < 0.f )
		{
			fCos	= -fCos;
			fSign	= -1.f;
		}

		const float fSin = 1.f - fCos*fCos;

		if ( fSin > fQuatEpsilon )
		{
			/*fSin = sqrt( fSin );
			const float fSinAngle		= 1.f / fSin;*/
			const float fSinAngle		= fastInvSqrt( fSin );
			const float fAngle			= atan2( fSin, fCos );
			const float fAngleByT		= fAngle * t;
			const float fSinAngleBySign = fSinAngle * fSign;

			const float fLowerWeight = sin( fAngle - fAngleByT )	* fSinAngle;
			const float fUpperWeight = sin( fAngleByT )				* fSinAngleBySign;

			w = ( w * (fLowerWeight) ) + ( q2.w * (fUpperWeight) );
			x = ( x * (fLowerWeight) ) + ( q2.x * (fUpperWeight) );
			y = ( y * (fLowerWeight) ) + ( q2.y * (fUpperWeight) );
			z = ( z * (fLowerWeight) ) + ( q2.z * (fUpperWeight) );
		}

		return *this;
	}

	//***************************************************************
	// quat_128
	//***************************************************************

	//__________________________________________________________________________
	const quat_128 quat_128::ZERO	= quat_128( 0.f, 0.f, 0.f, 1.f );
}
